#include "goto.h"

#include <atomic>
#include <mutex>

#include "dwarf_utils.h"
#include "elog.h"
#include "elog_file.h"
#include "iTips.h"
#include "ws_constant.h"
#include "ws_response.h"
#include "timer_utils.h"

using Clock = std::chrono::system_clock;

AstroTarget* Goto::target_ = nullptr;
DeepSkyObject Goto::dso_;
SolarGalaxyObject Goto::sgo_;

std::atomic<StateNotify> Goto::gotoer_state_ = STATE_IDLE;
std::atomic<StateNotify> Goto::tracker_state_ = STATE_IDLE;

std::mutex gotoer_state_mutex;
std::mutex tracker_state_mutex;  

extern std::atomic<int> delta_x;
extern std::atomic<int> delta_y;

float offset_frequency_yaw = 0.0;
float offset_frequency_pitch = 0.0;
float offset_speed_yaw = 0.0;
float offset_speed_pitch = 0.0;

int Goto::gotoAndTarckTarget() {
    b_goto = true;
    b_tracking = true;
    
    EquatorialCoordinates eqCoord;
    HourAngleCoordinates haCoord;

    // 更新当前倾斜坐标和机械坐标
    int ret = updateNowCoord();
    if (ret < 0) {
        b_reverse_motor_pitch_direction_ = false;// 默认正面GOTO
        setAndNotifyGotoerState(STATE_ASTRO_PLATE_SOLVING);

        // 无法获取电机位置，直接Plate Solving获取当前倾斜坐标
        ret = plateSolver_.start(eqCoord, haCoord);
        if (ret < 0) {
            if (isSolarGalaxyObject()) {
                itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, CODE_ASTRO_GOTO_READ_MOTOR_POSITION_AND_PLATE_SOLVING_FAILED);
                wsCommonResponse(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_PLATE_SOLVING_FAILED);
                wsCommonResponse(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_GOTO_FAILED);
            } else {
                itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_DSO, CODE_ASTRO_GOTO_READ_MOTOR_POSITION_AND_PLATE_SOLVING_FAILED);
                wsCommonResponse(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_PLATE_SOLVING_FAILED);
                wsCommonResponse(CMD_ASTRO_START_GOTO_DSO, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_GOTO_FAILED);
            }

            setAndNotifyGotoerState(STATE_STOPPED);
            setAndNotifyGotoerState(STATE_IDLE);

            return -CODE_ASTRO_GOTO_FAILED;// GOTO失败
        }

        auto now = Clock::now();
        DeepSkyObject(eqCoord, target_->getZenithCoord(), "").myCoordAtTime(now, now_my_coord_);
    }

    // 计算考虑回程的第一次GOTO的终点
    gotoTargetConsiderBacklash();

    // 设置电机参数，第一次GOTO快
    MotorParameter motor_yaw_para(0.0, motor_high_speed_, YAW_LEFT, motor_high_speed_resolution_level_, motor_speed_ramping_);
    MotorParameter motor_pitch_para(0.0, motor_high_speed_, PITCH_DOWN, motor_high_speed_resolution_level_, motor_speed_ramping_);
    MotorPath path(motor_yaw_para, motor_pitch_para);

    if (b_goto == false) {
        if (isSolarGalaxyObject()) {
            if (itips_observer_->getErrorCode(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM) == -1)
                itips_observer_->backToOldCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM);
        } else {
            if (itips_observer_->getErrorCode(CMD_ASTRO_START_GOTO_DSO) == -1)
                itips_observer_->backToOldCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_DSO);
        }

        wsCommonResponse(CMD_ASTRO_STOP_GOTO, MODULE_ASTRO, CMD_TYPE_RESPONSE, WS_OK);
        setAndNotifyGotoerState(STATE_STOPPED);
        setAndNotifyGotoerState(STATE_IDLE);

        return 0;
    }

    // 根据第一次GOTO的终点，规划路径
    ret = pathPlanning(now_my_coord_, path);
    if (ret < 0) {// 目标在限位内，或者撞限位
        if (isSolarGalaxyObject()) {
            itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, -ret);
            wsCommonResponse(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, MODULE_ASTRO, CMD_TYPE_RESPONSE, ret);
            wsCommonResponse(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_GOTO_FAILED);
        } else {
            itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_DSO, -ret);
            wsCommonResponse(CMD_ASTRO_START_GOTO_DSO, MODULE_ASTRO, CMD_TYPE_RESPONSE, ret);
            wsCommonResponse(CMD_ASTRO_START_GOTO_DSO, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_GOTO_FAILED);
        }

        setAndNotifyGotoerState(STATE_STOPPED);
        setAndNotifyGotoerState(STATE_IDLE);

        return ret;
    }

    now_my_coord_ = target_my_coord_;

    // 设置电机参数，第二次GOTO慢，减少回程
    path.motor_yaw_para.speed = motor_low_speed_;
    path.motor_yaw_para.resolution_level = motor_low_speed_resolution_level_;
    path.motor_pitch_para.speed = motor_low_speed_;
    path.motor_pitch_para.resolution_level = motor_low_speed_resolution_level_;

    if (b_goto == false) {
        if (isSolarGalaxyObject()) {
            if (itips_observer_->getErrorCode(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM) == -1)
                itips_observer_->backToOldCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM);
        } else {
            if (itips_observer_->getErrorCode(CMD_ASTRO_START_GOTO_DSO) == -1)
                itips_observer_->backToOldCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_DSO);
        }

        wsCommonResponse(CMD_ASTRO_STOP_GOTO, MODULE_ASTRO, CMD_TYPE_RESPONSE, WS_OK);
        setAndNotifyGotoerState(STATE_STOPPED);
        setAndNotifyGotoerState(STATE_IDLE);

        return 0;
    }

    // 第二次GOTO目标，考虑电机运动时间
    ret = gotoTargetConsiderMotorRunTime(now_my_coord_, path);
    if (ret < 0) {// 撞限位
        if (isSolarGalaxyObject()) {
            itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, -ret);
            wsCommonResponse(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, MODULE_ASTRO, CMD_TYPE_RESPONSE, ret);
            wsCommonResponse(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_GOTO_FAILED);
        } else {
            itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_DSO, -ret);
            wsCommonResponse(CMD_ASTRO_START_GOTO_DSO, MODULE_ASTRO, CMD_TYPE_RESPONSE, ret);
            wsCommonResponse(CMD_ASTRO_START_GOTO_DSO, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_GOTO_FAILED);
        }

        setAndNotifyGotoerState(STATE_STOPPED);
        setAndNotifyGotoerState(STATE_IDLE);

        return ret;
    }

    if (b_goto == false) {
        if (isSolarGalaxyObject()) {
            if (itips_observer_->getErrorCode(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM) == -1)
                itips_observer_->backToOldCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM);
        } else {
            if (itips_observer_->getErrorCode(CMD_ASTRO_START_GOTO_DSO) == -1)
                itips_observer_->backToOldCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_DSO);
        }

        wsCommonResponse(CMD_ASTRO_STOP_GOTO, MODULE_ASTRO, CMD_TYPE_RESPONSE, WS_OK);
        setAndNotifyGotoerState(STATE_STOPPED);
        setAndNotifyGotoerState(STATE_IDLE);

        return 0;
    }

    setAndNotifyGotoerState(STATE_ASTRO_PLATE_SOLVING);

    // Plate Solving计算当前位置
    auto now = Clock::now();
    ret = plateSolver_.start(eqCoord, haCoord);
    if (ret < 0) {
        now_my_coord_ = target_my_coord_;
        // 失败不提示
        // if (isSolarGalaxyObject()) 
        //     wsCommonResponse(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_PLATE_SOLVING_FAILED);
        // else
        //     wsCommonResponse(CMD_ASTRO_START_GOTO_DSO, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_PLATE_SOLVING_FAILED);

    } else {
        DeepSkyObject(eqCoord, target_->getZenithCoord(), "").myCoordAtTime(now, now_my_coord_);// 得到plate solving开始时的dwarf坐标系坐标

        // 更新倾斜坐标与机械坐标的对应关系
        last_my_coord_ = now_my_coord_;
        motor_yaw_.getAbsolutePositionInDegree(motor_high_speed_resolution_level_, last_mechanical_coord_.azi);
        motor_pitch_.getAbsolutePositionInDegree(motor_high_speed_resolution_level_, last_mechanical_coord_.alt);
    }

    if (b_goto == false) {
        if (isSolarGalaxyObject()) {
            if (itips_observer_->getErrorCode(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM) == -1)
                itips_observer_->backToOldCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM);
        } else {
            if (itips_observer_->getErrorCode(CMD_ASTRO_START_GOTO_DSO) == -1)
                itips_observer_->backToOldCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_DSO);
        }

        wsCommonResponse(CMD_ASTRO_STOP_GOTO, MODULE_ASTRO, CMD_TYPE_RESPONSE, WS_OK);
        setAndNotifyGotoerState(STATE_STOPPED);
        setAndNotifyGotoerState(STATE_IDLE);

        return 0;
    }

    // 第三次GOTO目标，微调
    ret = gotoTargetConsiderMotorRunTime(now_my_coord_, path);
    if (ret < 0) {// 撞限位
        if (isSolarGalaxyObject()) {
            itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, -ret);
            wsCommonResponse(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, MODULE_ASTRO, CMD_TYPE_RESPONSE, ret);
            wsCommonResponse(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_GOTO_FAILED);
        } else {
            itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_DSO, -ret);
            wsCommonResponse(CMD_ASTRO_START_GOTO_DSO, MODULE_ASTRO, CMD_TYPE_RESPONSE, ret);
            wsCommonResponse(CMD_ASTRO_START_GOTO_DSO, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_GOTO_FAILED);
        }

        setAndNotifyGotoerState(STATE_STOPPED);
        setAndNotifyGotoerState(STATE_IDLE);

        return ret;
    }

    if (b_goto == false) {
        if (isSolarGalaxyObject()) {
            if (itips_observer_->getErrorCode(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM) == -1)
                itips_observer_->backToOldCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM);
        } else {
            if (itips_observer_->getErrorCode(CMD_ASTRO_START_GOTO_DSO) == -1)
                itips_observer_->backToOldCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_DSO);
        }

        wsCommonResponse(CMD_ASTRO_STOP_GOTO, MODULE_ASTRO, CMD_TYPE_RESPONSE, WS_OK);
        setAndNotifyGotoerState(STATE_STOPPED);
        setAndNotifyGotoerState(STATE_IDLE);

        return 0;
    }

    setAndNotifyGotoerState(STATE_STOPPED);
    setAndNotifyGotoerState(STATE_IDLE);

    if (isSolarGalaxyObject()) {
        itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, WS_OK);    
    } else {
        itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_DSO, WS_OK);    
    }

    setAndNotifyTrackerState(STATE_RUNNING);

    rgb_power_.setRgbEffect(RGB_EFFECT_WORKING_STATE);

    offset_frequency_yaw = 0.0;
    offset_frequency_pitch = 0.0;

    delta_x = 0;
    delta_y = 0;

    // 开始跟踪
    while(b_tracking) {
        ret = trackTarget();
        if (ret < 0) {// 跟星撞限位
            if (isSolarGalaxyObject()) {
                itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, -ret);
                wsCommonResponse(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, MODULE_ASTRO, CMD_TYPE_RESPONSE, ret);
                wsCommonResponse(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_GOTO_FAILED);
            } else {
                itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_DSO, -ret);
                wsCommonResponse(CMD_ASTRO_START_GOTO_DSO, MODULE_ASTRO, CMD_TYPE_RESPONSE, ret);
                wsCommonResponse(CMD_ASTRO_START_GOTO_DSO, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_GOTO_FAILED);
            }

            setAndNotifyTrackerState(STATE_STOPPED);
            setAndNotifyTrackerState(STATE_IDLE);

            return ret;
        }
    }

    wsCommonResponse(CMD_ASTRO_STOP_GOTO, MODULE_ASTRO, CMD_TYPE_RESPONSE, WS_OK);

    setAndNotifyTrackerState(STATE_STOPPED);
    setAndNotifyTrackerState(STATE_IDLE);

    setTargetToNull();

    return 0;
}

int Goto::sunMoonHoughCircles(const cv::Mat& img, std::vector<cv::Vec3f>& circles) {
    const double adjust_mean_value = 200.0;

    if (img.empty()) {
        std::cerr << "Failed to read the image." << std::endl;
        return -1;
    }

    try {
        // 转换为灰度图像
        cv::Mat gray;
        cv::cvtColor(img, gray, cv::COLOR_BGR2GRAY);

        // 阈值处理
        cv::Mat blurred;
        cv::threshold(gray, blurred, 0, 255, cv::THRESH_TOZERO | cv::THRESH_TRIANGLE);

        // 计算均值
        cv::Scalar mean_value = cv::mean(blurred, blurred);

        // 乘以常数
        blurred = blurred * (adjust_mean_value / mean_value[0]);

        // 中值滤波
        cv::medianBlur(blurred, blurred, 21);

        // 双边滤波
        cv::Mat result;
        cv::bilateralFilter(blurred, result, 20, 200, 200);

        // Hough 圆检测
        cv::HoughCircles(result, circles, cv::HOUGH_GRADIENT, 1, 100, 100, 30, 0, 0);

        for (size_t i = 0; i < circles.size(); ++i) {
            cv::Vec3i c = circles[i];
            log_i("Detected circle: Center (%d, %d), Radius: %d", c[0], c[1], c[2]);
        }
    } catch (cv::Exception& e) {
        const char* err_msg = e.what();
        log_i("OpenCV exception caught:%s\n", err_msg);
        return -1;
    }

    return 0;
}

int Goto::getSunMoonCenterPos(cv::Vec3f& circle) {
    cv::Mat img;
    MEDIA_BUFFER dst_mb = cam_tele_.getRgbMB_4k(1920, 1080, true);
    if (dst_mb == NULL) {
        cv::Mat processed_raw;
        getProcessedRaw(processed_raw);  // 拿处理后的RAW图

        if (processed_raw.empty()) {
            return -1;
        }

        cv::resize(processed_raw, img, cv::Size(1920, 1080));
    } else {
        img = cv::Mat(1080, 1920, CV_8UC3, (unsigned char*)RK_MPI_MB_GetPtr(dst_mb));
    }

    std::vector<cv::Vec3f> circles;
    sunMoonHoughCircles(img, circles);

    rkfree(dst_mb);

    if (circles.size() == 1) {
        circle = circles[0];
        circle[0] = circle[0] * 3840.0 / img.cols;  // 转成4K坐标输出
        circle[1] = circle[1] * 2160.0 / img.rows;
        return 0;
    } else {
        log_i("circles.size():%d", circles.size());
        return -2;
    }
}

// 天文模式日月跟踪接口
int Goto::trackTargetConsiderBacklash() {
    b_tracking = true;

    // 设置电机参数，第一次GOTO快
    MotorParameter motor_yaw_para(0.0, motor_low_speed_, YAW_LEFT, motor_low_speed_resolution_level_, motor_speed_ramping_);
    MotorParameter motor_pitch_para(0.0, motor_low_speed_, PITCH_DOWN, motor_low_speed_resolution_level_, motor_speed_ramping_);
    MotorPath path(motor_yaw_para, motor_pitch_para);

    auto now = Clock::now();
    target_->myCoordAtTime(now, now_my_coord_);

    // 计算考虑回程的第一次GOTO的终点
    gotoTargetConsiderBacklash();

    int ret = pathPlanning(now_my_coord_, path);
    if (ret == -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED) {// 目标撞限位，不判断限位预警，因为不强制复位，电机位置可能错误导致误判
        if (isSolarGalaxyObject()) {
            itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, -ret);
            wsCommonResponse(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, MODULE_ASTRO, CMD_TYPE_RESPONSE, ret);
            wsCommonResponse(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_GOTO_FAILED);
        } else {
            itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_DSO, -ret);
            wsCommonResponse(CMD_ASTRO_START_GOTO_DSO, MODULE_ASTRO, CMD_TYPE_RESPONSE, ret);
            wsCommonResponse(CMD_ASTRO_START_GOTO_DSO, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_GOTO_FAILED);
        }

        setAndNotifyGotoerState(STATE_STOPPED);
        setAndNotifyGotoerState(STATE_IDLE);

        return ret;
    }

    now_my_coord_ = target_my_coord_;

    // 第二次GOTO目标，考虑电机运动时间
    ret = gotoTargetConsiderMotorRunTime(now_my_coord_, path);
    if (ret < 0) {// 撞限位
        if (isSolarGalaxyObject()) {
            itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, -ret);
            wsCommonResponse(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, MODULE_ASTRO, CMD_TYPE_RESPONSE, ret);
            wsCommonResponse(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_GOTO_FAILED);
        } else {
            itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_DSO, -ret);
            wsCommonResponse(CMD_ASTRO_START_GOTO_DSO, MODULE_ASTRO, CMD_TYPE_RESPONSE, ret);
            wsCommonResponse(CMD_ASTRO_START_GOTO_DSO, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_GOTO_FAILED);
        }

        setAndNotifyGotoerState(STATE_STOPPED);
        setAndNotifyGotoerState(STATE_IDLE);

        return ret;
    }

    setAndNotifyGotoerState(STATE_STOPPED);
    setAndNotifyGotoerState(STATE_IDLE);

    if (isSolarGalaxyObject()) {
        itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, WS_OK);    
    } else {
        itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_DSO, WS_OK);    
    }

    setAndNotifyTrackerState(STATE_RUNNING);

    rgb_power_.setRgbEffect(RGB_EFFECT_WORKING_STATE);

    offset_speed_yaw = 0.0;
    offset_speed_pitch = 0.0;

    delta_x = 0;
    delta_y = 0;

    std::thread track_offset_thread([&]() {
        bool b_first_time = true;
        cv::Vec3f first_circle;

        time_checker timer;

        const int offset_pix_threshold = 10;

        float kp_x = 1.0;
        float kp_y = 1.0;

        while (b_tracking) {
            timer.start();

            HorizontalCoordinates target_coord;
            auto now = Clock::now();
            target_->myCoordAtTime(now, target_coord);

            cv::Vec3f circle;
            int ret = getSunMoonCenterPos(circle);
            if (ret < 0) {
                offset_speed_yaw = 0;  // 如果检测失败，将补偿参数置零，避免无法反馈导致主动漂移
                offset_speed_pitch = 0;

                if (ret == -1)
                    sleep(1);  // 拿不到图

                continue;
            }

            if (b_first_time) {
                first_circle = circle;
                b_first_time = false;
            } else {
                timer.stop();
                timer.show_distance("detect sun moon cost ms");
                float detect_sun_moon_time = timer.distance() / 1000.0;

                delta_x = first_circle[0] - circle[0];
                delta_y = first_circle[1] - circle[1];

                // offset_pix_threshold像素以外的偏移保持offset_pix_threshold像素，控制修正幅度
                if (delta_x > offset_pix_threshold) {
                    circle[0] = (first_circle[0] - offset_pix_threshold) < 0 ? 0 : (first_circle[0] - offset_pix_threshold);
                } else if (delta_x < -offset_pix_threshold) {
                    circle[0] = (first_circle[0] + offset_pix_threshold) > 3840 ? 3840 : (first_circle[0] + offset_pix_threshold);
                }

                if (delta_y > offset_pix_threshold) {
                    circle[1] = (first_circle[1] - offset_pix_threshold) < 0 ? 0 : (first_circle[1] - offset_pix_threshold);
                } else if (delta_y < -offset_pix_threshold) {
                    circle[1] = (first_circle[1] + offset_pix_threshold) > 2160 ? 2160 : (first_circle[1] + offset_pix_threshold);
                }

                // 粗略假设中心机械坐标是太阳坐标，严谨应该根据太阳坐标和像素坐标反求中心机械坐标
                HorizontalCoordinates first_sun_moon_coord;
                teleViewPixelCoordToMechanicalCoord(cv::Point2i(first_circle[0], first_circle[1]), first_sun_moon_coord, target_coord);  // 要求4K坐标

                HorizontalCoordinates sun_moon_coord;
                teleViewPixelCoordToMechanicalCoord(cv::Point2i(circle[0], circle[1]), sun_moon_coord, target_coord);

                if (b_tracking == false)
                    break;

                if (abs(delta_x) > offset_pix_threshold * 1.5) {
                    kp_x = 1.0;
                } else if (abs(delta_x) > offset_pix_threshold) {
                    kp_x = 0.5;
                } else if (abs(delta_x) > offset_pix_threshold / 2) {
                    kp_x = 0.3;
                }

                if (abs(delta_y) > offset_pix_threshold * 1.5) {
                    kp_y = 1.0;
                } else if (abs(delta_y) > offset_pix_threshold) {
                    kp_y = 0.5;
                } else if (abs(delta_y) > offset_pix_threshold / 2) {
                    kp_y = 0.3;
                }

                offset_speed_yaw = (sun_moon_coord.azi - first_sun_moon_coord.azi) / detect_sun_moon_time * kp_x;
                offset_speed_pitch = (sun_moon_coord.alt - first_sun_moon_coord.alt) / detect_sun_moon_time * kp_y;
            }
        }
    });
    track_offset_thread.detach();

    // 开始跟踪
    while(b_tracking) {
        int ret = trackSunMoon();
        if (ret < 0) {// 跟星撞限位
            if (isSolarGalaxyObject()) {
                itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, -ret);
                wsCommonResponse(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, MODULE_ASTRO, CMD_TYPE_RESPONSE, ret);
                wsCommonResponse(CMD_ASTRO_START_GOTO_SOLAR_SYSTEM, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_GOTO_FAILED);
            } else {
                itips_observer_->updateCmdIdAndErrorCode(CMD_ASTRO_START_GOTO_DSO, -ret);
                wsCommonResponse(CMD_ASTRO_START_GOTO_DSO, MODULE_ASTRO, CMD_TYPE_RESPONSE, ret);
                wsCommonResponse(CMD_ASTRO_START_GOTO_DSO, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_GOTO_FAILED);
            }

            setAndNotifyTrackerState(STATE_STOPPED);
            setAndNotifyTrackerState(STATE_IDLE);

            return ret;
        }
    }

    wsCommonResponse(CMD_ASTRO_STOP_GOTO, MODULE_ASTRO, CMD_TYPE_RESPONSE, WS_OK);

    setAndNotifyTrackerState(STATE_STOPPED);
    setAndNotifyTrackerState(STATE_IDLE);

    setTargetToNull();

    return 0;
}

// 其他模式日月跟踪接口
int Goto::trackSpecialTarget() {
    b_tracking = true;

    // 设置电机参数，第一次GOTO快
    MotorParameter motor_yaw_para(0.0, motor_low_speed_, YAW_LEFT, motor_low_speed_resolution_level_, motor_speed_ramping_);
    MotorParameter motor_pitch_para(0.0, motor_low_speed_, PITCH_DOWN, motor_low_speed_resolution_level_, motor_speed_ramping_);
    MotorPath path(motor_yaw_para, motor_pitch_para);

    auto now = Clock::now();
    target_->myCoordAtTime(now, now_my_coord_);

    // 计算考虑回程的第一次GOTO的终点
    gotoTargetConsiderBacklash();

    int ret = pathPlanning(now_my_coord_, path);
    if (ret == -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED) {// 目标撞限位，不判断限位预警，因为不强制复位，电机位置可能错误导致误判
        wsCommonResponse(CMD_ASTRO_START_TRACK_SPECIAL_TARGET, MODULE_ASTRO, CMD_TYPE_RESPONSE, ret);
        wsCommonResponse(CMD_ASTRO_START_TRACK_SPECIAL_TARGET, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_GOTO_FAILED);

        setAndNotifySpecialTrackerState(STATE_STOPPED);
        setAndNotifySpecialTrackerState(STATE_IDLE);

        return ret;
    }

    now_my_coord_ = target_my_coord_;

    // 第二次GOTO目标，考虑电机运动时间
    ret = gotoTargetConsiderMotorRunTime(now_my_coord_, path);
    if (ret < 0) {// 撞限位
        wsCommonResponse(CMD_ASTRO_START_TRACK_SPECIAL_TARGET, MODULE_ASTRO, CMD_TYPE_RESPONSE, ret);
        wsCommonResponse(CMD_ASTRO_START_TRACK_SPECIAL_TARGET, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_GOTO_FAILED);

        setAndNotifySpecialTrackerState(STATE_STOPPED);
        setAndNotifySpecialTrackerState(STATE_IDLE);

        return ret;
    }

    offset_speed_yaw = 0.0;
    offset_speed_pitch = 0.0;

    delta_x = 0;
    delta_y = 0;

    std::thread track_offset_thread([&]() {
        bool b_first_time = true;
        cv::Vec3f first_circle;

        time_checker timer;

        const int offset_pix_threshold = 10;

        float kp_x = 1.0;
        float kp_y = 1.0;

        while (b_tracking) {
            timer.start();

            HorizontalCoordinates target_coord;
            auto now = Clock::now();
            target_->myCoordAtTime(now, target_coord);

            cv::Vec3f circle;
            if (getSunMoonCenterPos(circle) < 0) {
                offset_speed_yaw = 0;  // 如果检测失败，将补偿参数置零，避免无法反馈导致主动漂移
                offset_speed_pitch = 0;

                continue;
            }

            if (b_first_time) {
                first_circle = circle;
                b_first_time = false;
            } else {
                timer.stop();
                timer.show_distance("detect sun moon cost ms");
                float detect_sun_moon_time = timer.distance() / 1000.0;

                delta_x = first_circle[0] - circle[0];
                delta_y = first_circle[1] - circle[1];

                // offset_pix_threshold像素以外的偏移保持offset_pix_threshold像素，控制修正幅度
                if (delta_x > offset_pix_threshold) {
                    circle[0] = (first_circle[0] - offset_pix_threshold) < 0 ? 0 : (first_circle[0] - offset_pix_threshold);
                } else if (delta_x < -offset_pix_threshold) {
                    circle[0] = (first_circle[0] + offset_pix_threshold) > 3840 ? 3840 : (first_circle[0] + offset_pix_threshold);
                }

                if (delta_y > offset_pix_threshold) {
                    circle[1] = (first_circle[1] - offset_pix_threshold) < 0 ? 0 : (first_circle[1] - offset_pix_threshold);
                } else if (delta_y < -offset_pix_threshold) {
                    circle[1] = (first_circle[1] + offset_pix_threshold) > 2160 ? 2160 : (first_circle[1] + offset_pix_threshold);
                }

                // 粗略假设中心机械坐标是太阳坐标，严谨应该根据太阳坐标和像素坐标反求中心机械坐标
                HorizontalCoordinates first_sun_moon_coord;
                teleViewPixelCoordToMechanicalCoord(cv::Point2i(first_circle[0], first_circle[1]), first_sun_moon_coord, target_coord);  // 要求4K坐标

                HorizontalCoordinates sun_moon_coord;
                teleViewPixelCoordToMechanicalCoord(cv::Point2i(circle[0], circle[1]), sun_moon_coord, target_coord);

                if (b_tracking == false)
                    break;

                if (abs(delta_x) > offset_pix_threshold * 1.5) {
                    kp_x = 1.0;
                } else if (abs(delta_x) > offset_pix_threshold) {
                    kp_x = 0.5;
                } else if (abs(delta_x) > offset_pix_threshold / 2) {
                    kp_x = 0.3;
                }

                if (abs(delta_y) > offset_pix_threshold * 1.5) {
                    kp_y = 1.0;
                } else if (abs(delta_y) > offset_pix_threshold) {
                    kp_y = 0.5;
                } else if (abs(delta_y) > offset_pix_threshold / 2) {
                    kp_y = 0.3;
                }

                offset_speed_yaw = (sun_moon_coord.azi - first_sun_moon_coord.azi) / detect_sun_moon_time * kp_x;
                offset_speed_pitch = (sun_moon_coord.alt - first_sun_moon_coord.alt) / detect_sun_moon_time * kp_y;
            }
        }
    });
    track_offset_thread.detach();

    // 开始跟踪
    while(b_tracking) {
        int ret = trackSunMoon();
        if (ret < 0) {// 跟星撞限位
            wsCommonResponse(CMD_ASTRO_START_TRACK_SPECIAL_TARGET, MODULE_ASTRO, CMD_TYPE_RESPONSE, ret);
            wsCommonResponse(CMD_ASTRO_START_TRACK_SPECIAL_TARGET, MODULE_ASTRO, CMD_TYPE_RESPONSE, -CODE_ASTRO_GOTO_FAILED);

            setAndNotifySpecialTrackerState(STATE_STOPPED);
            setAndNotifySpecialTrackerState(STATE_IDLE);

            return ret;
        }
    }

    wsCommonResponse(CMD_ASTRO_STOP_TRACK_SPECIAL_TARGET, MODULE_ASTRO, CMD_TYPE_RESPONSE, WS_OK);

    setAndNotifySpecialTrackerState(STATE_STOPPED);
    setAndNotifySpecialTrackerState(STATE_IDLE);

    setTargetToNull();

    return 0;
}

int Goto::stopGotoAndTracking() {
    if (gotoer_state_ == STATE_RUNNING || gotoer_state_ == STATE_ASTRO_PLATE_SOLVING) 
        setAndNotifyGotoerState(STATE_STOPPING);
    else if (gotoer_state_ != STATE_IDLE)
        setAndNotifyGotoerState(gotoer_state_);
    else if (tracker_state_ == STATE_RUNNING) 
        setAndNotifyTrackerState(STATE_STOPPING);
    else if (tracker_state_ != STATE_IDLE)
        setAndNotifyTrackerState(tracker_state_);

    if (gotoer_state_ == STATE_IDLE && tracker_state_ == STATE_IDLE) {
        setAndNotifyGotoerState(STATE_IDLE);
        setAndNotifyTrackerState(STATE_IDLE);
        wsCommonResponse(CMD_ASTRO_STOP_GOTO, MODULE_ASTRO, CMD_TYPE_RESPONSE, WS_OK);
    }

    b_goto = false;
    b_tracking = false;
    
    plateSolver_.stop();
    motor_yaw_.stop();
    motor_pitch_.stop();

    return 0;
}

int Goto::stopGoto() {
    b_goto = false;
    
    plateSolver_.stop();
    motor_yaw_.stop();
    motor_pitch_.stop();

    return 0;
}

int Goto::stopTrackingSpecial() {
    if (tracker_state_ == STATE_RUNNING) 
        setAndNotifySpecialTrackerState(STATE_STOPPING);
    else 
        setAndNotifySpecialTrackerState(tracker_state_);

    if (tracker_state_ == STATE_IDLE)
        wsCommonResponse(CMD_ASTRO_STOP_TRACK_SPECIAL_TARGET, MODULE_ASTRO, CMD_TYPE_RESPONSE, WS_OK);

    b_tracking = false;
    
    motor_yaw_.stop();
    motor_pitch_.stop();

    return 0;
}

int Goto::updateNowCoord() {
    int ret = motor_yaw_.getAbsolutePositionInDegree(motor_high_speed_resolution_level_, now_mechanical_coord_.azi);
    if (ret == 0) {
        ret = motor_pitch_.getAbsolutePositionInDegree(motor_high_speed_resolution_level_, now_mechanical_coord_.alt);
        if (ret == 0) {
            if (b_reverse_motor_yaw_mechanical_direction_) {
                now_my_coord_.azi = LIMIT_DEGREES_TO_360(last_my_coord_.azi - (now_mechanical_coord_.azi - last_mechanical_coord_.azi));  // 方位轴机械坐标反，向右复位
            } else {
                now_my_coord_.azi = LIMIT_DEGREES_TO_360(last_my_coord_.azi + (now_mechanical_coord_.azi - last_mechanical_coord_.azi));
            }

            if (b_reverse_motor_pitch_mechanical_direction_) {  // 俯仰轴机械坐标反，向上复位
                if (last_mechanical_coord_.alt + motor_pitch_up_reset_offset_angle < 180) {// 开始在反面，向上复位硬件补偿不同
                    log_i("FROM BACKSIDE!");
                    double direct_now_my_coord_alt;
                    direct_now_my_coord_alt = last_my_coord_.alt + (now_mechanical_coord_.alt - last_mechanical_coord_.alt);// 机械坐标反
                    now_my_coord_.alt = LIMIT_DEC_TO_90(direct_now_my_coord_alt);

                    log_i("direct_now_my_coord_alt:%f", direct_now_my_coord_alt);
                    if (direct_now_my_coord_alt > 90) {// 俯仰当前相对上次有翻转
                        log_i("TO FRONTSIDE!");
                        b_reverse_motor_pitch_direction_ = false;
                        now_my_coord_.azi = LIMIT_DEGREES_TO_360(now_my_coord_.azi + 180);
                    } else {
                        log_i("TO BACKSIDE!");
                        b_reverse_motor_pitch_direction_ = true;
                    }
                } else {
                    log_i("FROM FRONTSIDE!");
                    double direct_now_my_coord_alt;
                    direct_now_my_coord_alt = last_my_coord_.alt - (now_mechanical_coord_.alt - last_mechanical_coord_.alt);// 机械坐标反
                    now_my_coord_.alt = LIMIT_DEC_TO_90(direct_now_my_coord_alt);

                    log_i("direct_now_my_coord_alt:%f", direct_now_my_coord_alt);
                    if (direct_now_my_coord_alt > 90) {// 俯仰当前相对上次有翻转
                        log_i("TO BACKSIDE!");
                        b_reverse_motor_pitch_direction_ = true;
                        now_my_coord_.azi = LIMIT_DEGREES_TO_360(now_my_coord_.azi + 180);
                    } else {
                        log_i("TO FRONTSIDE!");
                        b_reverse_motor_pitch_direction_ = false;
                    }
                }
            } else {
                if (last_mechanical_coord_.alt + motor_pitch_down_reset_offset_angle < 180) {  // 开始在正面
                    log_i("FROM FRONTSIDE!");
                    double direct_now_my_coord_alt;
                    direct_now_my_coord_alt = last_my_coord_.alt + (now_mechanical_coord_.alt - last_mechanical_coord_.alt);  // 正面加上角度差
                    now_my_coord_.alt = LIMIT_DEC_TO_90(direct_now_my_coord_alt);

                    log_i("direct_now_my_coord_alt:%f", direct_now_my_coord_alt);
                    if (direct_now_my_coord_alt > 90) {  // 俯仰当前相对上次有翻转
                        log_i("TO BACKSIDE!");
                        b_reverse_motor_pitch_direction_ = true;
                        now_my_coord_.azi = LIMIT_DEGREES_TO_360(now_my_coord_.azi + 180);
                    } else {
                        log_i("TO FRONTSIDE!");
                        b_reverse_motor_pitch_direction_ = false;
                    }
                } else {  // 开始在反面
                    log_i("FROM BACKSIDE!");
                    double direct_now_my_coord_alt;
                    direct_now_my_coord_alt = last_my_coord_.alt - (now_mechanical_coord_.alt - last_mechanical_coord_.alt);  // 反面减去角度差
                    now_my_coord_.alt = LIMIT_DEC_TO_90(direct_now_my_coord_alt);

                    log_i("direct_now_my_coord_alt:%f", direct_now_my_coord_alt);
                    if (direct_now_my_coord_alt > 90) {  // 俯仰当前相对上次有翻转
                        log_i("TO FRONTSIDE!");
                        b_reverse_motor_pitch_direction_ = false;
                        now_my_coord_.azi = LIMIT_DEGREES_TO_360(now_my_coord_.azi + 180);
                    } else {
                        log_i("TO BACKSIDE!");
                        b_reverse_motor_pitch_direction_ = true;
                    }
                }
            }
            
            last_mechanical_coord_ = now_mechanical_coord_;
            last_my_coord_ = now_my_coord_;

            return 0;
        } else {
            return -1;
        }
    } else {
        return -1;
    }
}

int Goto::gotoTargetConsiderBacklash() {
    HorizontalCoordinates target_my_coord_now;
    HorizontalCoordinates target_my_coord_after_1min;

    auto now = Clock::now();
    target_->myCoordAtTime(now, target_my_coord_now);

    Clock::time_point time_point_after_1_min = now + std::chrono::minutes(1);
    target_->myCoordAtTime(time_point_after_1_min, target_my_coord_after_1min);

    double delta_azi = target_my_coord_after_1min.azi - target_my_coord_now.azi;
    delta_azi = abs(delta_azi) > 180 ? (delta_azi > 0 ? delta_azi - 360 : delta_azi + 360) : delta_azi;
    double azi_backlash = RADIANS_TO_DEGREES(acos((COSD(MotorController::motor_yaw_backlash_) - SIND(target_my_coord_now.alt) * SIND(target_my_coord_now.alt)) / (COSD(target_my_coord_now.alt) * COSD(target_my_coord_now.alt))));
    target_my_coord_.azi = delta_azi > 0 ? 
                        LIMIT_DEGREES_TO_360(target_my_coord_now.azi - azi_backlash) : 
                        LIMIT_DEGREES_TO_360(target_my_coord_now.azi + azi_backlash);

    double delta_alt = target_my_coord_after_1min.alt - target_my_coord_now.alt;
    target_my_coord_.alt = delta_alt > 0 ? 
                        (target_my_coord_now.alt - MotorController::motor_pitch_backlash_ < -90 ? -90 : target_my_coord_now.alt - MotorController::motor_pitch_backlash_) : 
                        (target_my_coord_now.alt + MotorController::motor_pitch_backlash_ > 90 ? 90 :  target_my_coord_now.alt + MotorController::motor_pitch_backlash_);

    log_i("target_my_coord_(%f, %f)", target_my_coord_.azi, target_my_coord_.alt);

    return 0;
}

int Goto::pathPlanning(const HorizontalCoordinates& now_my_coord, MotorPath& path) {
    path.motor_yaw_para.angle = abs(target_my_coord_.azi - now_my_coord.azi);
    path.motor_yaw_para.direction = target_my_coord_.azi - now_my_coord.azi > 0 ? YAW_RIGHT : YAW_LEFT;

    path.motor_pitch_para.angle = abs(target_my_coord_.alt - now_my_coord.alt);
    path.motor_pitch_para.direction = target_my_coord_.alt - now_my_coord.alt > 0 ? PITCH_UP : PITCH_DOWN;

    int ret = 0;
    double end_position = 0;
    for (int i = 0; i < 2; i++) {
        if (motor_yaw_.getEndPositionInDegree(path.motor_yaw_para.angle, path.motor_yaw_para.resolution_level, path.motor_yaw_para.direction, end_position) >= 0) {
            ret = motor_yaw_.checkPositionInDegree(end_position);
            if (ret != 0) {
                path.motor_yaw_para.angle = 360 - path.motor_yaw_para.angle;
                path.motor_yaw_para.direction = !path.motor_yaw_para.direction; 
            } else {
                break;
            }
        } else {
            break;
        }
    }

    if (ret != 0) {  // 目标在限位内
        if (b_reverse_motor_pitch_direction_) {  // 在反面，转到正面
            path.motor_pitch_para.direction = PITCH_DOWN;
            b_reverse_motor_pitch_direction_ = false;
        } else {  // 在正面，转到反面
            path.motor_pitch_para.direction = PITCH_UP;
            b_reverse_motor_pitch_direction_ = true;
        }

        path.motor_pitch_para.angle = 2 * (90 - now_my_coord.alt);

        motor_pitch_.setParameterInDegree(path.motor_pitch_para.angle, path.motor_pitch_para.speed, path.motor_pitch_para.resolution_level, path.motor_pitch_para.speed_ramping, path.motor_pitch_para.direction);
        motor_pitch_.run();
        motor_pitch_.waitMotorToStop();

        now_my_coord_.azi = LIMIT_DEGREES_TO_360(now_my_coord_.azi + 180);

        return pathPlanning(now_my_coord_, path);
    }

    if (b_reverse_motor_pitch_direction_)
        path.motor_pitch_para.direction = !path.motor_pitch_para.direction;

    motor_yaw_.setParameterInDegree(path.motor_yaw_para.angle, path.motor_yaw_para.speed, path.motor_yaw_para.resolution_level, path.motor_yaw_para.speed_ramping, path.motor_yaw_para.direction);
    motor_pitch_.setParameterInDegree(path.motor_pitch_para.angle, path.motor_pitch_para.speed, path.motor_pitch_para.resolution_level, path.motor_pitch_para.speed_ramping, path.motor_pitch_para.direction);

    motor_yaw_.run();
    motor_pitch_.run();

    if (motor_yaw_.waitMotorToStop() == -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED)
        ret = -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED;
    if (motor_pitch_.waitMotorToStop() == -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED)
        ret = -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED;

    return ret;
}

int Goto::gotoTargetConsiderMotorRunTime(const HorizontalCoordinates& now_my_coord, MotorPath& path) {
    auto now = Clock::now();
    target_->myCoordAtTime(now, target_my_coord_);

    float yaw_angle = target_my_coord_.azi - now_my_coord.azi;
    float rotate_angle = std::abs(yaw_angle) > 180 ? (int(yaw_angle) > 0 ? yaw_angle - 360 : yaw_angle + 360) : yaw_angle;

    path.motor_yaw_para.angle = abs(rotate_angle);
    path.motor_yaw_para.direction = rotate_angle > 0 ? YAW_RIGHT : YAW_LEFT;

    path.motor_pitch_para.angle = abs(target_my_coord_.alt - now_my_coord.alt);
    path.motor_pitch_para.direction = target_my_coord_.alt - now_my_coord.alt > 0 ? PITCH_UP : PITCH_DOWN;

    motor_yaw_.setParameterInDegree(path.motor_yaw_para.angle, path.motor_yaw_para.speed, path.motor_yaw_para.resolution_level, path.motor_yaw_para.speed_ramping, path.motor_yaw_para.direction);
    motor_pitch_.setParameterInDegree(path.motor_pitch_para.angle, path.motor_pitch_para.speed, path.motor_pitch_para.resolution_level, path.motor_pitch_para.speed_ramping, path.motor_pitch_para.direction);

    double motor_yaw_run_time = motor_yaw_.getRunTime();
    double motor_pitch_run_time = motor_pitch_.getRunTime();
    double motor_run_time = motor_yaw_run_time > motor_pitch_run_time ? motor_yaw_run_time : motor_pitch_run_time;
    double wait_exp_time = 2 * exposure_;// 电机停后，等待2个曝光时间，等待图像稳定

    Clock::duration duration_motor_run_time = std::chrono::duration_cast<Clock::duration>(std::chrono::duration<double>(motor_run_time + wait_exp_time));
    Clock::time_point time_point_after_motor_running = now + duration_motor_run_time;
    target_->myCoordAtTime(time_point_after_motor_running, target_my_coord_);

    yaw_angle = target_my_coord_.azi - now_my_coord.azi;
    rotate_angle = std::abs(yaw_angle) > 180 ? (int(yaw_angle) > 0 ? yaw_angle - 360 : yaw_angle + 360) : yaw_angle;

    path.motor_yaw_para.angle = abs(rotate_angle);
    path.motor_yaw_para.direction = rotate_angle > 0 ? YAW_RIGHT : YAW_LEFT;

    path.motor_pitch_para.angle = abs(target_my_coord_.alt - now_my_coord.alt);
    path.motor_pitch_para.direction = target_my_coord_.alt - now_my_coord.alt > 0 ? PITCH_UP : PITCH_DOWN;

    if (b_reverse_motor_pitch_direction_)
        path.motor_pitch_para.direction = !path.motor_pitch_para.direction;

    motor_yaw_.setParameterInDegree(path.motor_yaw_para.angle, path.motor_yaw_para.speed, path.motor_yaw_para.resolution_level, path.motor_yaw_para.speed_ramping, path.motor_yaw_para.direction);
    motor_pitch_.setParameterInDegree(path.motor_pitch_para.angle, path.motor_pitch_para.speed, path.motor_pitch_para.resolution_level, path.motor_pitch_para.speed_ramping, path.motor_pitch_para.direction);

    motor_yaw_.run();
    motor_pitch_.run();

    int ret = 0;
    if (motor_yaw_.waitMotorToStop() == -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED)
        ret = -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED;
    if (motor_pitch_.waitMotorToStop() == -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED)
        ret = -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED;

    sleep(wait_exp_time);

    return ret;
}

int Goto::teleViewPixelCoordToMechanicalCoord(const cv::Point& pixel_coord, HorizontalCoordinates& mechanical_coord, const HorizontalCoordinates& center_mechanical_coord) {
    const double cam_tele_f_ = 100000.0;// 理论焦距100mm，10^5um
    const double cam_tele_pix_size_ = 1.45;// 长焦像素大小1.45um 
    const double cam_tele_f_pix_ = cam_tele_f_ / cam_tele_pix_size_;// 长焦像素焦距

    const double cam_tele_resolution_x_ = 3840;// 与pix_size对应的分辨率
    const double cam_tele_resolution_y_ = 2160;
    const double cam_tele_center_pix_x_ = cam_tele_resolution_x_ / 2.0;
    const double cam_tele_center_pix_y_ = cam_tele_resolution_y_ / 2.0;

    // 像平面在三维空间的x轴上
    const cv::Mat cam_tele_matrix_pixel_to_camera_coord_ = (cv::Mat_<double>(3, 3) << 1.0, 0, 0, 
                                                                                    cam_tele_center_pix_x_, cam_tele_f_pix_, 0, 
                                                                                    cam_tele_center_pix_y_, 0, -cam_tele_f_pix_);

    cv::Mat pixel_coord_vector = (cv::Mat_<double>(3, 1) << 1.0, pixel_coord.x, pixel_coord.y);

    cv::Mat camera_coord_vector;
    camera_coord_vector = cam_tele_matrix_pixel_to_camera_coord_.inv() * pixel_coord_vector;

    // 相机坐标系下，图像边界的方位角和俯仰角都在(-π/2 <= atan() <= π/2)中
    double theta_camera_coord = atan(camera_coord_vector.at<double>(0, 1));
    double phi_camera_coord = atan(camera_coord_vector.at<double>(0, 2) * cos(theta_camera_coord));

    cv::Mat xyz_camera_coord = (cv::Mat_<double>(3, 1) << cos(phi_camera_coord) * cos(theta_camera_coord), 
                                                          cos(phi_camera_coord) * sin(theta_camera_coord), 
                                                          sin(phi_camera_coord));

    cv::Mat matrix_rotate_z = (cv::Mat_<double>(3, 3) << COSD(center_mechanical_coord.azi), SIND(center_mechanical_coord.azi), 0,
                                                        -SIND(center_mechanical_coord.azi), COSD(center_mechanical_coord.azi), 0,
                                                         0, 0, 1);

    cv::Mat matrix_rotate_y = (cv::Mat_<double>(3, 3) << COSD(center_mechanical_coord.alt), 0, SIND(center_mechanical_coord.alt),
                                                         0, 1, 0,
                                                        -SIND(center_mechanical_coord.alt), 0, COSD(center_mechanical_coord.alt));

    cv::Mat matrix_rotate = matrix_rotate_y * matrix_rotate_z;

    cv::Mat xyz_mechanical_coord = matrix_rotate.inv() * xyz_camera_coord;

    double r = cv::norm(xyz_mechanical_coord);//应该为1
    mechanical_coord.alt = RADIANS_TO_DEGREES(asin(xyz_mechanical_coord.at<double>(0, 2) / r));

    mechanical_coord.azi = RADIANS_TO_DEGREES(atan2(xyz_mechanical_coord.at<double>(0, 1), xyz_mechanical_coord.at<double>(0, 0)));
    mechanical_coord.azi = LIMIT_DEGREES_TO_360(mechanical_coord.azi);

    log_i("mechanical_coord:(%f, %f)", mechanical_coord.azi, mechanical_coord.alt);

    return 0;
}

int Goto::trackSunMoon() {
    now_my_coord_ = target_my_coord_;

    const int fresh_time_s = 1;
    auto now = Clock::now() + std::chrono::seconds(fresh_time_s);
    target_->myCoordAtTime(now, target_my_coord_);

    MotorParameter motor_yaw_para(0.0, 0.0, YAW_LEFT, motor_track_resolution_level_, motor_speed_ramping_);
    MotorParameter motor_pitch_para(0.0, 0.0, PITCH_DOWN, motor_track_resolution_level_, motor_speed_ramping_);

    int offset_x = delta_x;
    int offset_y = delta_y;

    const float step_angle = 1.8;                        
    const float gear_ratio = 61.0 * 31.0 / 2 / 17;
    const float resolution = 256.0;

    const float p2a_coefficient = step_angle / resolution / gear_ratio;

    float offset_frequency_yaw = offset_speed_yaw / p2a_coefficient;
    float offset_frequency_pitch = offset_speed_pitch / p2a_coefficient;

    if (b_reverse_motor_pitch_direction_) {
        offset_speed_yaw = -offset_speed_yaw;// 背面时，图像上左右反向
        offset_speed_pitch = -offset_speed_pitch;
    }

    // log_i("delta(%d, %d), offset_frequency(%f, %f), offset_speed(%f, %f)", offset_x, offset_y, offset_frequency_yaw, offset_frequency_pitch, offset_speed_yaw, offset_speed_pitch);

    float yaw_angle = target_my_coord_.azi - now_my_coord_.azi;
    float rotate_angle = std::abs(yaw_angle) > 180 ? (int(yaw_angle) > 0 ? yaw_angle - 360 : yaw_angle + 360) : yaw_angle;
    float yaw_speed = rotate_angle / fresh_time_s + offset_speed_yaw;

    float pitch_speed = (target_my_coord_.alt - now_my_coord_.alt) / fresh_time_s + offset_speed_pitch;

    motor_yaw_para.speed = std::abs(yaw_speed);
    motor_yaw_para.direction = yaw_speed > 0 ? YAW_RIGHT : YAW_LEFT;

    motor_pitch_para.speed = std::abs(pitch_speed);
    motor_pitch_para.direction = pitch_speed > 0 ? PITCH_UP : PITCH_DOWN;

    if (b_reverse_motor_pitch_direction_)
        motor_pitch_para.direction = !motor_pitch_para.direction;

    int ret = motor_yaw_.setParameterInDegree(motor_yaw_para.speed, motor_yaw_para.resolution_level, motor_yaw_para.speed_ramping, motor_yaw_para.direction);
    if (ret == -CODE_STEP_MOTOR_IS_RUNNING) {
        motor_yaw_.changeSpeed(motor_yaw_para.speed);
        motor_yaw_.changeDirection(motor_yaw_para.direction);
    } else {
        motor_yaw_.run();
    }

    ret = motor_pitch_.setParameterInDegree(motor_pitch_para.speed, motor_pitch_para.resolution_level, motor_pitch_para.speed_ramping, motor_pitch_para.direction);
    if (ret == -CODE_STEP_MOTOR_IS_RUNNING) {
        motor_pitch_.changeSpeed(motor_pitch_para.speed);
        motor_pitch_.changeDirection(motor_pitch_para.direction);
    } else {
        motor_pitch_.run();
    }

    std::this_thread::sleep_for(std::chrono::milliseconds(fresh_time_s * 1000));

    if (motor_yaw_.getLimitState() == STEP_MOTOR_LIMITED || motor_pitch_.getLimitState() == STEP_MOTOR_LIMITED)
        return -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED;

    return 0;
}

int Goto::trackTarget() {
    now_my_coord_ = target_my_coord_;

    const int fresh_time_s = 1;
    auto now = Clock::now() + std::chrono::seconds(fresh_time_s);
    target_->myCoordAtTime(now, target_my_coord_);

    MotorParameter motor_yaw_para(0.0, 0.0, YAW_LEFT, motor_track_resolution_level_, motor_speed_ramping_);
    MotorParameter motor_pitch_para(0.0, 0.0, PITCH_DOWN, motor_track_resolution_level_, motor_speed_ramping_);

    int offset_x = delta_x;
    int offset_y = delta_y;

    if(offset_x >= 50) 
        offset_frequency_yaw = -1.5;
    else if(offset_x >= 20) 
        offset_frequency_yaw = -1.0;
    else if(offset_x >= 8) 
        offset_frequency_yaw = -0.5;
    else if(offset_x >= -8) 
        offset_frequency_yaw += 0.0;
    else if(offset_x >= -20) 
        offset_frequency_yaw = 0.5;
    else if(offset_x >= -50) 
        offset_frequency_yaw = 1.0;
    else
        offset_frequency_yaw = 1.5;

    if(offset_y >= 50) 
        offset_frequency_pitch = 1.5;
    else if(offset_y >= 20) 
        offset_frequency_pitch = 1.0;
    else if(offset_y >= 8) 
        offset_frequency_pitch = 0.5;
    else if(offset_y >= -8) 
        offset_frequency_pitch += 0.0;
    else if(offset_y >= -20) 
        offset_frequency_pitch = -0.5;
    else if(offset_y >= -50) 
        offset_frequency_pitch = -1.0;
    else
        offset_frequency_pitch = -1.5;

    const float step_angle = 1.8;                        
    const float gear_ratio = 61.0 * 31.0 / 2 / 17;
    const float resolution = 256.0;

    const float p2a_coefficient = step_angle / resolution / gear_ratio;
    offset_speed_yaw = offset_frequency_yaw * p2a_coefficient;
    offset_speed_pitch = offset_frequency_pitch * p2a_coefficient;

    if (b_reverse_motor_pitch_direction_) {
        offset_speed_yaw = -offset_speed_yaw;// 背面时，图像上左右反向
        offset_speed_pitch = -offset_speed_pitch;
    }

    // log_i("delta(%d, %d), offset_frequency(%f, %f), offset_speed(%f, %f)", offset_x, offset_y, offset_frequency_yaw, offset_frequency_pitch, offset_speed_yaw, offset_speed_pitch);

    float yaw_angle = target_my_coord_.azi - now_my_coord_.azi;
    float rotate_angle = std::abs(yaw_angle) > 180 ? (int(yaw_angle) > 0 ? yaw_angle - 360 : yaw_angle + 360) : yaw_angle;
    float yaw_speed = rotate_angle / fresh_time_s + offset_speed_yaw;

    float pitch_speed = (target_my_coord_.alt - now_my_coord_.alt) / fresh_time_s + offset_speed_pitch;

    motor_yaw_para.speed = std::abs(yaw_speed);
    motor_yaw_para.direction = yaw_speed > 0 ? YAW_RIGHT : YAW_LEFT;

    motor_pitch_para.speed = std::abs(pitch_speed);
    motor_pitch_para.direction = pitch_speed > 0 ? PITCH_UP : PITCH_DOWN;

    if (b_reverse_motor_pitch_direction_)
        motor_pitch_para.direction = !motor_pitch_para.direction;

    int ret = motor_yaw_.setParameterInDegree(motor_yaw_para.speed, motor_yaw_para.resolution_level, motor_yaw_para.speed_ramping, motor_yaw_para.direction);
    if (ret == -CODE_STEP_MOTOR_IS_RUNNING) {
        motor_yaw_.changeSpeed(motor_yaw_para.speed);
        motor_yaw_.changeDirection(motor_yaw_para.direction);
    } else {
        motor_yaw_.run();
    }

    ret = motor_pitch_.setParameterInDegree(motor_pitch_para.speed, motor_pitch_para.resolution_level, motor_pitch_para.speed_ramping, motor_pitch_para.direction);
    if (ret == -CODE_STEP_MOTOR_IS_RUNNING) {
        motor_pitch_.changeSpeed(motor_pitch_para.speed);
        motor_pitch_.changeDirection(motor_pitch_para.direction);
    } else {
        motor_pitch_.run();
    }

    std::this_thread::sleep_for(std::chrono::milliseconds(fresh_time_s * 1000));

    if (motor_yaw_.getLimitState() == STEP_MOTOR_LIMITED || motor_pitch_.getLimitState() == STEP_MOTOR_LIMITED)
        return -CODE_STEP_MOTOR_LIMIT_POSITION_HITTED;

    return 0;
}

void SolarGalaxyObject::myCoordAtTime(const Clock::time_point& time_point, HorizontalCoordinates& myCoord) {
    time_t t = Clock::to_time_t(time_point);
    tm utc_time;
    gmtime_r(&t, &utc_time);

    auto msec = std::chrono::duration_cast<std::chrono::milliseconds>(time_point.time_since_epoch()).count() % 1000;
    double sec = utc_time.tm_sec + static_cast<double>(msec) / 1000.0;

    EquatorialCoordinates starEqCoordJNow = Ephemeris::solarSystemObjectEquatorialCoordinatesAtDateAndTime(target_Index_,
                                                                                        utc_time.tm_mday, utc_time.tm_mon + 1, utc_time.tm_year + 1900, 
                                                                                        utc_time.tm_hour, utc_time.tm_min, sec);

    HourAngleCoordinates starHaCoord = Ephemeris::equatorialToHACoordinatesAtDateAndTime(starEqCoordJNow,
                                                                                        utc_time.tm_mday, utc_time.tm_mon + 1, utc_time.tm_year + 1900, 
                                                                                        utc_time.tm_hour, utc_time.tm_min, sec);

    Ephemeris::haCoordToMyCoord(starHaCoord, zenith_coord_, myCoord);

    // log_i("day:%d, month:%d, year:%d, hour:%d, min:%d, sec:%f", utc_time.tm_mday, utc_time.tm_mon + 1, utc_time.tm_year + 1900, utc_time.tm_hour, utc_time.tm_min, sec);
    // log_i("planet:%d, zenith.ha:%f, zenith.dec:%f, mycoord.azi:%f, mycoord.alt:%f", target_Index_, zenith_coord_.ha, zenith_coord_.dec, myCoord.azi, myCoord.alt);
}

void DeepSkyObject::myCoordAtTime(const Clock::time_point& time_point, HorizontalCoordinates& myCoord) {
    time_t t = Clock::to_time_t(time_point);
    tm utc_time;
    gmtime_r(&t, &utc_time);

    auto msec = std::chrono::duration_cast<std::chrono::milliseconds>(time_point.time_since_epoch()).count() % 1000;
    double sec = utc_time.tm_sec + static_cast<double>(msec) / 1000.0;

    EquatorialCoordinates starEqCoordJNow = Ephemeris::equatorialEquinoxToEquatorialJNowAtDateAndTime(target_eq_coord_J2000_, 2000, 
                                                                                        utc_time.tm_mday, utc_time.tm_mon + 1, utc_time.tm_year + 1900, 
                                                                                        utc_time.tm_hour, utc_time.tm_min, sec);

    HourAngleCoordinates starHaCoord = Ephemeris::equatorialToHACoordinatesAtDateAndTime(starEqCoordJNow,
                                                                                        utc_time.tm_mday, utc_time.tm_mon + 1, utc_time.tm_year + 1900, 
                                                                                        utc_time.tm_hour, utc_time.tm_min, sec);

    Ephemeris::haCoordToMyCoord(starHaCoord, zenith_coord_, myCoord);

    // log_i("day:%d, month:%d, year:%d, hour:%d, min:%d, sec:%f", utc_time.tm_mday, utc_time.tm_mon + 1, utc_time.tm_year + 1900, utc_time.tm_hour, utc_time.tm_min, sec);
    // log_i("zenith.ha:%f, zenith.dec:%f, mycoord.azi:%f, mycoord.alt:%f", zenith_coord_.ha, zenith_coord_.dec, myCoord.azi, myCoord.alt);
}

// 使用线程锁保护获取的状态和主动通知的状态之间的顺序，确保后面的状态不会先发送
void Goto::setAndNotifyGotoerState(const StateNotify& state) {
    std::unique_lock<std::mutex> lock(gotoer_state_mutex);

    gotoer_state_ = state;

    log_i("gotoer_state_:%d", gotoer_state_.load());

    wsResNotifyAstroGotoState(CMD_NOTIFY_STATE_ASTRO_GOTO, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, gotoer_state_);
}

void Goto::notifyGotoerState() {
    std::unique_lock<std::mutex> lock(gotoer_state_mutex);

    if (gotoer_state_ != STATE_IDLE) {
        log_i("gotoer_state_:%d", gotoer_state_.load());
        wsResNotifyAstroGotoState(CMD_NOTIFY_STATE_ASTRO_GOTO, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, gotoer_state_);
    }
}

void Goto::setAndNotifyTrackerState(const StateNotify& state) {
    std::unique_lock<std::mutex> lock(tracker_state_mutex);

    std::string target_name;
    getTargetName(target_name);

    tracker_state_ = state;

    log_i("tracker_state_:%d, target_name:%s", tracker_state_.load(), target_name.c_str());

    wsResNotifyAstroTrackingState(CMD_NOTIFY_STATE_ASTRO_TRACKING, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, tracker_state_, target_name);
}

void Goto::notifyTrackerState() {
    std::unique_lock<std::mutex> lock(tracker_state_mutex);

    if (tracker_state_ != STATE_IDLE) {
        std::string target_name;
        getTargetName(target_name);

        log_i("tracker_state_:%d, target_name:%s", tracker_state_.load(), target_name.c_str());

        wsResNotifyAstroTrackingState(CMD_NOTIFY_STATE_ASTRO_TRACKING, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, tracker_state_, target_name);
    }
}

void Goto::setAndNotifySpecialTrackerState(const StateNotify& state) {
    std::unique_lock<std::mutex> lock(tracker_state_mutex);

    std::string target_name;
    getTargetName(target_name);

    int index = -1;
    if (target_name == "Sun")
        index = 0;
    else if (target_name == "Moon")
        index = 1;

    tracker_state_ = state;

    log_i("tracker_state_:%d, target_name:%s, index:%d", tracker_state_.load(), target_name.c_str(), index);

    wsResNotifyAstroSpecialTrackingState(CMD_NOTIFY_STATE_ASTRO_TRACKING_SPECIAL, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, tracker_state_, target_name, index);
}

void Goto::notifySpecialTrackerState() {
    std::unique_lock<std::mutex> lock(tracker_state_mutex);

    if (tracker_state_ != STATE_IDLE) {
        std::string target_name;
        getTargetName(target_name);

        int index = -1;
        if (target_name == "Sun")
            index = 0;
        else if (target_name == "Moon")
            index = 1;

        log_i("tracker_state_:%d, target_name:%s, index:%d", tracker_state_.load(), target_name.c_str(), index);

        wsResNotifyAstroSpecialTrackingState(CMD_NOTIFY_STATE_ASTRO_TRACKING_SPECIAL, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, tracker_state_, target_name, index);
    }
}